#include <stdio.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <math.h>
#include <time.h>
#include <sstream>

#define PATHLENGTH 20
#define LEARNING_TIME 9

#define FRAME_WIDTH	640
#define FRAME_HEIGHT	480
//#define FRAME_PIXNB	FRAME_WIDTH*FRAME_HEIGHT

#define CAPTURE_DELTA	30

#define FONTSIZE_MAINMESSAGE	1.0
#define FONTSIZE_DATA			0.5

#define DETECTION_COLORBOUND	50
#define DETECTION_FACTOR		1

#define HUE_MAX	256
#define SAT_MAX 256
#define VAL_MAX 256


int GetStats(int histSize, int* in_values,int& out_mean, int& out_stdDev)
{
	// calculates poundered mean
	// NOTE the type might be too short
	long mean =0;
	long total=0;
	for(int i=0;i<(histSize);i++)
	{
		mean += in_values[i]*i;
		total+= in_values[i]; // could be calculated outside
	}
	if(total!=0)	mean /= total;
	out_mean = mean;
	
	//Racine carrée [somme (xi - m)² / (n-1)]
	long stdDev = 0;
	for(int i=0;i<(histSize);i++)
	{
		//int sqval = in_values[i] - mean;
		long sqval = in_values[i]*i - mean;
		sqval /= (total-1);
		stdDev += sqval*sqval ;
	}
	//stdDev /= (histSize-1);
	//stdDev /= (total-1);
	stdDev = sqrt(stdDev);
	
	out_stdDev = stdDev;
	
	return 0;
}


void AddPathToImage(CvPoint* path, int pathLength, IplImage* img) {
	
	for(int i = 1; i < pathLength ; i++) {
		if((path[i-1].x > 0) && (path[i].x > 0))
		{
			cvLine(img, path[i-1], path[i], cvScalar(0,255,255), 5);
		}
	} 

}

void AddPointToPath(CvPoint point, CvPoint* path, int pathLength) {
	for(int i = pathLength; i > 0 ; i--) {
		path[i] = path[i-1];
	}
	path[0] = point;
}

int GetHistogram(IplImage* in_img, CvPoint in_topleft, CvPoint in_bottoright, int* out_hue , int* out_sat, int* out_val) {
	
	printf("building histograms");
	//int max_index = max(HUE_MAX,max(SAT_MAX,VAL_MAX));
	int max_index = 360;
	for(int z=0;z<max_index;z++)
	{
		if(z<HUE_MAX) out_hue[z] = 0;
		if(z<SAT_MAX) out_sat[z] = 0;
		if(z<VAL_MAX) out_val[z] = 0;
	}
	
	int x1 = in_topleft.x;;
	int y1 = in_topleft.y;
	
	int x2 = in_bottoright.x;
	int y2 = in_bottoright.y; 

	for(int i=x1;i<=x2;i++) 
	{
		for(int j=y1;j<=y2;j++) 
		{
			int hue = CV_IMAGE_ELEM(in_img,uchar,j,i*3);
			//printf("H=%d ", hue);
			out_hue[hue]++;
			int sat = CV_IMAGE_ELEM(in_img,uchar,j,i*3+1); 
			//printf("S=%d ",sat);
			out_sat[sat]++;
			int val = CV_IMAGE_ELEM(in_img,uchar,j,i*3+2); 
			//printf("V=%d ",val);
			out_val[val]++;
			//printf("\n");
		}
	}
	
	
	return 0;
}


int DrawHistogram(int* histogram, int width, CvPoint in_topleft, CvPoint in_bottomright, CvScalar color ,IplImage* out_img)
{

	//printf("drawing histogram ");
	int tickness = 1; 
			
	int x1 = in_topleft.x;;
	int y1 = in_topleft.y;
	
	int x2 = in_bottomright.x;
	int y2 = in_bottomright.y;
	
	int h = x2-x1;
	int w = y2-y1;
	
	for(int i=0;i<width;i++)
	{
		int x = x1+i*w/width;
		int y = y2-histogram[i]*h/width;
		cvLine(out_img, cvPoint(x, y2), cvPoint(x, y), color, tickness);
		 
	} 

	//cvAdd(out_img, imgHistogram, out_img);
	
	//delete &imgHistogram;
	return 0;
}


IplImage* GetThresholdedImage(IplImage* img, CvScalar lowerColorBound, CvScalar upperColorBound)
{
	//Convert the image into an HSV image
	IplImage* imgHSV = cvCreateImage(cvGetSize(img), 8, 3);
	cvCvtColor(img,imgHSV,CV_BGR2HSV);

	IplImage* imgThreshed = cvCreateImage(cvGetSize(img), 8, 1);
	
	cvInRangeS(imgHSV, lowerColorBound, upperColorBound, imgThreshed);
	
	
	cvRelease((void**)&imgHSV);
	return imgThreshed;
}





int main (int argc, const char * argv[]) {

	CvPoint* path = new CvPoint[PATHLENGTH];
	for(int i=0;i<PATHLENGTH;i++) path[i] = cvPoint(-1, -1);

	// Initialize capturing live feed from the camera
    CvCapture* capture = 0;
    capture = cvCaptureFromCAM(0);
 
    // Couldn't get a device? Throw an error and quit
    if(!capture)
    {
        printf("Could not initialize capturing...\n");
        return -1;
    }

    // The 1rst windows we'll be using
    cvNamedWindow("video");
	cvMoveWindow("video", 0, 0);

	
	
    // This image holds the "scribble" data...
    // the tracked positions of the ball
    IplImage* imgScribble = NULL;
	IplImage* videoRender = NULL;
	IplImage* imgHSV = NULL;
	
	
	// Learning color to track
	bool learningIsOk = false;
	//cvDisplayStatusBar("video", "Learning...", 100);
	/* initialize font */
    CvFont fontLearning;
    cvInitFont(&fontLearning, CV_FONT_HERSHEY_SIMPLEX, FONTSIZE_MAINMESSAGE, FONTSIZE_MAINMESSAGE, 0, 1, CV_AA);
	/* initialize font for data */
    CvFont fontDataSmall;
    cvInitFont(&fontDataSmall, CV_FONT_HERSHEY_SIMPLEX, FONTSIZE_DATA, FONTSIZE_DATA, 0, 1, CV_AA);

	int hueHistogram[HUE_MAX];
	int satHistogram[SAT_MAX];
	int valHistogram[VAL_MAX];

	time_t endTime = time(NULL)+LEARNING_TIME;
	
	
	
    while(!learningIsOk)
    {
		time_t currentTime = time(NULL);
		
		// Will hold a frame captured from the camera
        IplImage* frame = 0;
        frame = cvQueryFrame(capture);
		cvFlip(frame,NULL,1);
		
		
		if(videoRender == NULL)
		{
			videoRender = cvCreateImage(cvGetSize(frame), 8, 3);
		}

        // If we couldn't grab a frame... quit
        if(!frame)
            break;

        // If this is the first frame, we need to initialize it
        if(imgScribble == NULL)
        {
            imgScribble = cvCreateImage(cvGetSize(frame), 8, 3);
        }


		 
        // Add the scribbling image and the frame...
        cvAdd(frame, imgScribble, frame);
		std::stringstream sstext;
		sstext << "Learning... " << (endTime-currentTime) << " s";
		std::string stext = sstext.str();
		cvPutText(frame, stext.c_str(), cvPoint(10, 30), &fontLearning, cvScalar(255, 255, 255, 0));
		
		// convert capture to HSV
		imgHSV = cvCreateImage(cvGetSize(frame), 8, 3);
		cvCvtColor(frame,imgHSV,CV_BGR2HSV);
		
		// Get hue histogram
		GetHistogram(imgHSV, cvPoint(320-CAPTURE_DELTA, 240-CAPTURE_DELTA), cvPoint(320+CAPTURE_DELTA, 240+CAPTURE_DELTA), hueHistogram, satHistogram, valHistogram);
		DrawHistogram(hueHistogram, HUE_MAX, cvPoint(10, 50), cvPoint(138, 150), cvScalar(255,100,100), frame);
		DrawHistogram(satHistogram, SAT_MAX, cvPoint(10, 50), cvPoint(138, 150), cvScalar(100,255,100), frame);
		DrawHistogram(valHistogram, VAL_MAX, cvPoint(10, 50), cvPoint(138, 150), cvScalar(100,100,255), frame);
		
		// Draw the detection frame using 
		int hueMean, satMean, valMean;
		int hueStdDev, satStdDev, valStdDev;
		GetStats(HUE_MAX,hueHistogram, hueMean, hueStdDev);
		GetStats(SAT_MAX,satHistogram, satMean, satStdDev);
		GetStats(VAL_MAX,valHistogram, valMean, valStdDev);

		int tickness = 2;
		CvScalar meanColorHSV = cvScalar(hueMean,satMean,valMean); 
		CvScalar fluo = cvScalar(100,255,100); 
		 cvLine(frame, cvPoint(320-CAPTURE_DELTA, 240-CAPTURE_DELTA), cvPoint(320-CAPTURE_DELTA, 240+CAPTURE_DELTA), fluo, tickness);
		 cvLine(frame, cvPoint(320-CAPTURE_DELTA, 240+CAPTURE_DELTA), cvPoint(320+CAPTURE_DELTA, 240+CAPTURE_DELTA), fluo, tickness);
		 cvLine(frame, cvPoint(320+CAPTURE_DELTA, 240+CAPTURE_DELTA), cvPoint(320+CAPTURE_DELTA, 240-CAPTURE_DELTA), fluo, tickness);
		 cvLine(frame, cvPoint(320+CAPTURE_DELTA, 240-CAPTURE_DELTA), cvPoint(320-CAPTURE_DELTA, 240-CAPTURE_DELTA), fluo, tickness);
		// draw a dot with the detected color
		cvLine(frame, cvPoint(320+CAPTURE_DELTA+10, 240-CAPTURE_DELTA-10), cvPoint(320+CAPTURE_DELTA+10, 240-CAPTURE_DELTA-10), fluo, 22);
		cvLine(frame, cvPoint(320+CAPTURE_DELTA+10, 240-CAPTURE_DELTA-10), cvPoint(320+CAPTURE_DELTA+10, 240-CAPTURE_DELTA-10), meanColorHSV, 20);
		
		
		
		// Add the mean color values to the frame
        cvAdd(frame, imgScribble, frame);
		std::stringstream ssHue, ssSat, ssVal;
		ssHue << "H = " << hueMean << " (" << hueStdDev << ")";
		ssSat << "S = " << satMean << " (" << satStdDev << ")";
		ssVal << "V = " << valMean << " (" << valStdDev << ")";
		std::string tHue = ssHue.str();
		std::string tSat = ssSat.str();
		std::string tval = ssVal.str();
		//cvPutText(frame, stext.c_str(), cvPoint(10, 30), &fontLearning, cvScalar(255, 255, 255, 0));
		cvPutText(frame, tHue.c_str(), cvPoint(320+CAPTURE_DELTA+10, 240-CAPTURE_DELTA+10), &fontDataSmall, cvScalar(255, 255, 255, 0));
		cvPutText(frame, tSat.c_str(), cvPoint(320+CAPTURE_DELTA+10, 240-CAPTURE_DELTA+30), &fontDataSmall, cvScalar(255, 255, 255, 0));
		cvPutText(frame, tval.c_str(), cvPoint(320+CAPTURE_DELTA+10, 240-CAPTURE_DELTA+50), &fontDataSmall, cvScalar(255, 255, 255, 0));
		
		
		
        //cvShowImage("thresh", imgYellowThresh);
        cvShowImage("video", frame);

        // Wait for a keypress
        int c = cvWaitKey(10);
        if(c!=-1)
        {
            // If pressed, break out of the loop
            break;
        }
	
		if(currentTime>=endTime)	learningIsOk = true;
	}
	
	
	int hueMean, satMean, valMean;
	int hueStdDev, satStdDev, valStdDev;
	GetStats(HUE_MAX,hueHistogram, hueMean, hueStdDev);
	GetStats(SAT_MAX,satHistogram, satMean, satStdDev);
	GetStats(VAL_MAX,valHistogram, valMean, valStdDev);
	//CvScalar lowerColorBound = cvScalar(hueMean-DETECTION_COLORBOUND,satMean-DETECTION_COLORBOUND,valMean-DETECTION_COLORBOUND);
	//CvScalar upperColorBound = cvScalar(hueMean+DETECTION_COLORBOUND,satMean+DETECTION_COLORBOUND,valMean+DETECTION_COLORBOUND);
	CvScalar lowerColorBound = cvScalar(hueMean-DETECTION_FACTOR*hueStdDev,satMean-DETECTION_FACTOR*satStdDev,valMean-DETECTION_FACTOR*valStdDev);
	CvScalar upperColorBound = cvScalar(hueMean+DETECTION_FACTOR*hueStdDev,satMean+DETECTION_FACTOR*satStdDev,valMean+DETECTION_FACTOR*valStdDev);
	CvScalar meanColorHSV =  cvScalar(hueMean,satMean,valMean);
	
	
	// The 2nd window
	cvNamedWindow("thresh");
	cvMoveWindow("thresh", FRAME_WIDTH, 0);

    // An infinite loop
    while(TRUE)
    {
        // Will hold a frame captured from the camera
        IplImage* frame = 0;
        frame = cvQueryFrame(capture);
		cvFlip(frame,NULL,1);

        // If we couldn't grab a frame... quit
        if(!frame)
            break;

        // If this is the first frame, we need to initialize it
        if(imgScribble == NULL)
        {
            imgScribble = cvCreateImage(cvGetSize(frame), 8, 3);
        }
		if(videoRender == NULL)
		{
			videoRender = cvCreateImage(cvGetSize(frame), 8, 3);
		}

        // Holds the yellow thresholded image (meanColor = white, rest = black)
        IplImage* imgYellowThresh = GetThresholdedImage(frame,lowerColorBound,upperColorBound);

        // Calculate the moments to estimate the position of the ball
        CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
        cvMoments(imgYellowThresh, moments, 1);
 
        // The actual moment values
        double moment10 = cvGetSpatialMoment(moments, 1, 0);
        double moment01 = cvGetSpatialMoment(moments, 0, 1);
        double area = cvGetCentralMoment(moments, 0, 0);


        // Holding the last and current ball positions
        static int posX = 0;
        static int posY = 0;
 
        int lastX = posX;
        int lastY = posY;
 
        posX = moment10/area;
        posY = moment01/area;

        // Print it out for debugging purposes
        printf("position (%d,%d)\n", posX, posY);
		
        //// We want to draw a line only if its a valid position
        //if(lastX>0 && lastY>0 && posX>0 && posY>0)
        //{
        //    // Draw a yellow line from the previous point to the current point
        //    cvLine(imgScribble, cvPoint(posX, posY), cvPoint(lastX, lastY), cvScalar(0,255,255), 5);
		//	cvCircle(imgScribble, cvPoint(posX,posY), 10, cvScalar(255,0,0), 5);
		//}
		AddPointToPath(cvPoint(posX,posY),path,PATHLENGTH);
		
		

        // Add the scribbling image and the frame...
        //cvAdd(frame, imgScribble, frame);
		cvPutText(frame, "Tracking", cvPoint(10, 30), &fontLearning, cvScalar(255, 255, 255, 0));
		AddPathToImage(path, PATHLENGTH, frame);
		
		// Add the mean color values to the frame
        cvAdd(frame, imgScribble, frame);
		std::stringstream ssHue, ssSat, ssVal;
		ssHue << "H = " << hueMean << " (" << hueStdDev << ")";
		ssSat << "S = " << satMean << " (" << satStdDev << ")";
		ssVal << "V = " << valMean << " (" << valStdDev << ")";
		std::string tHue = ssHue.str();
		std::string tSat = ssSat.str();
		std::string tval = ssVal.str();
		//cvPutText(frame, stext.c_str(), cvPoint(10, 30), &fontLearning, cvScalar(255, 255, 255, 0));
		cvPutText(frame, tHue.c_str(), cvPoint(10, 50+10), &fontDataSmall, cvScalar(255, 255, 255, 0));
		cvPutText(frame, tSat.c_str(), cvPoint(10, 50+30), &fontDataSmall, cvScalar(255, 255, 255, 0));
		cvPutText(frame, tval.c_str(), cvPoint(10, 50+50), &fontDataSmall, cvScalar(255, 255, 255, 0));
		
		//CvScalar meanColorRVB;
		//cvtColor(meanColorHSV, meanColorRVB, CV_HSV2BGR);
		cvLine(frame, cvPoint(20,50+80), cvPoint(20,50+80), meanColorHSV, 20);
		
		
		
        cvShowImage("thresh", imgYellowThresh);
        cvShowImage("video", frame);

        // Wait for a keypress
        int c = cvWaitKey(10);
        if(c!=-1)
        {
            // If pressed, break out of the loop
            break;
        }

        // Release the thresholded image+moments... we need no memory leaks.. please
        cvReleaseImage(&imgYellowThresh);
        delete moments;
    }


    // We're done using the camera. Other applications can now use it
    cvReleaseCapture(&capture);
    return 0;
}






